#include <iostream>
#include "serial/serial.h"
#include "iostream"
#include "vector"
#include "string"

using namespace serial;
using namespace std;
bool _readSerialDataFlag = true;
const string ZERO_STR = "0";
const string SPLIT_DELIMITER = "aa550399";

/**
 * 陀螺仪l两个字节uint8字节转 int16
 * @param lowByte
 * @param highByte
 * @return int16
 */
int16_t bytesToInt16(uint8_t lowByte, uint8_t highByte) {
    return static_cast<int16_t>(lowByte | (highByte << 8));
}

/**
 * 传感器数据解析数据返回的结构体
 */
struct SensorData {
    uint8_t frameType;//帧类型
    uint8_t sensorType;  //手套类型 1-左右  2-右手
    uint8_t power;  //电量 0-100
    uint8_t matrixData[256];  //传感器原始数据
    int8_t roll, pitch, yaw; //陀螺仪俯仰角 偏航叫 旋转角
};
/**
 * 传感器数据解析
 * @param originData 串口读取原始字节数组
 * @return 解析后的传感器数据
 */
SensorData parseSensorData(vector<int> originData) {
    SensorData sensorData{};
    if (originData.size()>=262){
        sensorData.frameType = static_cast<uint8_t>(originData.at(0));
        sensorData.sensorType = static_cast<uint8_t>(originData.at(1));
        sensorData.power = static_cast<uint8_t>(originData.at(2));

        for (int i = 3; i < originData.size() - 6; ++i) {
            sensorData.matrixData[i - 3] = static_cast<uint8_t>( originData.at(i));
        }
        sensorData.roll=bytesToInt16(static_cast<int8_t>( originData.at(259)),static_cast<int8_t>( originData.at(260)));
        sensorData.pitch=bytesToInt16(static_cast<int8_t>( originData.at(261)),static_cast<int8_t>( originData.at(262)));
        sensorData.yaw=bytesToInt16(static_cast<int8_t>( originData.at(263)),static_cast<int8_t>( originData.at(264)));
//        int count=0;
//        for (int i = 0; i <sizeof (sensorData.matrixData); ++i) {
//            printf(" %d ",sensorData.matrixData[i]);
//        }
//        printf("count:%d\r\n",count);
        printf("\r\n");
        printf("frameType:%d  sensorType:%d  power:%d  roll:%d pitch:%d  yaw:%d    \r\n",sensorData.frameType,sensorData.sensorType,sensorData.power,sensorData.pitch,sensorData.roll ,sensorData.yaw);
    }
    return sensorData;
}

int main() {
    std::vector<PortInfo> portList = serial::list_ports();
    for (auto port: portList) {
        printf("port:%s\r\n", port.port.c_str());
    }
    string portName="COM10";
    #if 0
        std:cout<<"please input serialport : "<<endl;
        std::cin>>portName;
        std::cout<<"port name is:"<<portName;
    #endif
    serial::Serial *my_serial = new serial::Serial(portName, 1000000, serial::Timeout::simpleTimeout(1000));
    string bufferStr;
    while (_readSerialDataFlag) {
        int _availBuf = my_serial->available();
        if (_availBuf > 0) {
            string data = my_serial->read(_availBuf);
            for (uint8_t mychar: data) {
                stringstream ss;
                ss << hex << ((int) mychar);
                if (ss.str().length() == 1) {
                    bufferStr.append(ZERO_STR).append(ss.str());
                } else {
                    bufferStr.append(ss.str());
                }
                int position = bufferStr.find(SPLIT_DELIMITER);
                if (position != -1) {
                    vector<int> matrixArr;
                    string serialDataSub = bufferStr.substr(0, position);
                    for (int i = 0; i < serialDataSub.length() / 2; i++) {
                        string substring = serialDataSub.substr(i * 2, 2);
                        stringstream deciamlStr;
                        int a;
                        deciamlStr << hex << substring;
                        deciamlStr >> a;
                        matrixArr.push_back(a);
                    }
                    bufferStr.erase(0, position + 8);
                    parseSensorData(matrixArr);
                }
            }
        }
    }
    return 0;
}
